#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"

#include <utility>
#include <vector>

#include "Eigen/Core"
#include "cartographer/common/internal/ceres_solver_options.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        namespace scan_matching
        {
            proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D(common::LuaParameterDictionary *const parameter_dictionary)
            {
                proto::CeresScanMatcherOptions2D options;
                options.set_occupied_space_weight(parameter_dictionary->GetDouble("occupied_space_weight"));
                options.set_translation_weight(parameter_dictionary->GetDouble("translation_weight"));
                options.set_rotation_weight(parameter_dictionary->GetDouble("rotation_weight"));
                *options.mutable_ceres_solver_options() =
                    common::CreateCeresSolverOptionsProto(parameter_dictionary->GetDictionary("ceres_solver_options").get());
                return options;
            }

            CeresScanMatcher2D::CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D &options)
                : options_(options)
                , ceres_solver_options_(common::CreateCeresSolverOptions(options.ceres_solver_options()))
            {
                ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
            }

            CeresScanMatcher2D::~CeresScanMatcher2D()
            {
            }

            void CeresScanMatcher2D::Match(const Eigen::Vector2d &target_translation, const transform::Rigid2d &init_pose,
                                           const sensor::PointCloud &point_cloud, const Grid2D &grid,
                                           transform::Rigid2d *const pose_estimate, ceres::Solver::Summary *const summary) const
            {
                ceres::Problem problem;
                CHECK_GT(options_.occupied_space_weight(), 0.);
                double ceres_pose[3] = {init_pose.translation().x(), init_pose.translation().y(), init_pose.rotation().angle()};
                double point_weight = options_.occupied_space_weight() / std::sqrt(static_cast<double>(point_cloud.size()));
                switch (grid.GetGridType())
                {
                case GridType::PROBABILITY_GRID: {
                    auto *cost = CreateOccupiedSpaceCostFunction2D(point_weight, point_cloud, grid);
                    problem.AddResidualBlock(cost, nullptr /* loss function */, ceres_pose);
                    break;
                }
                case GridType::TSDF: {
                    auto *cost = CreateTSDFMatchCostFunction2D(point_weight, point_cloud, static_cast<const TSDF2D &>(grid));
                    problem.AddResidualBlock(cost, nullptr /* loss function */, ceres_pose);
                    break;
                }
                }
                CHECK_GT(options_.translation_weight(), 0.);
                problem.AddResidualBlock(
                    TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.translation_weight(), target_translation),
                    nullptr /* loss function */, ceres_pose);
                CHECK_GT(options_.rotation_weight(), 0.);
                problem.AddResidualBlock(RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.rotation_weight(), ceres_pose[2]),
                                         nullptr /* loss function */, ceres_pose);
                ceres::Solve(ceres_solver_options_, &problem, summary);
                *pose_estimate = transform::Rigid2d({ceres_pose[0], ceres_pose[1]}, ceres_pose[2]);
                // auto cov_pose = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();
                // ceres::Covariance::Options cov_options;
                // ceres::Covariance covariance(cov_options);
                // std::vector<std::pair<const double *, const double *>> covariance_blocks;
                // covariance_blocks.push_back(std::make_pair(ceres_pose, ceres_pose));
                // covariance.Compute(covariance_blocks, &problem);
                // covariance.GetCovarianceBlockInTangentSpace(ceres_pose, ceres_pose, cov_pose.data());
            }
        } // namespace scan_matching
    }     // namespace mapping
} // namespace cartographer
